"""
car_dynamatic_model.py      2023.3.29
超平小车运动学模型
以二自由度模型为基础
"""

import math

L = 1.18  # 小车轴距
thr_a = 3  # 0.1油门产生0.3g的加速度
brake_a = 2  # 0.1制动产生0.2g的制动减速度
Max_a = 6  # 整车最大加速度，受限于路面环境与轮胎附着力
steer_gap = 4  # 转向拉杆间隙（角度制）

class vehicle:
    def __init__(self, x=0.0, y=0.0, ho=0.0, v=0.0):
        self.x = x
        self.y = y
        self.ho = ho
        self.ho_out = 0.0
        self.v = v
        self.thr = 0.0
        self.brake = 0.0
        self.steer = 0.0
        self.model = 2          # replay model
        self.dt = 1.0 / 60.0
        self.soc = 99
        self.a = 0.0
        self.dev_angle = - 3.0  # 小车偏移角
        self.bef_steer = 0.0    # 记录上次转向角度
        self.bef_s = [-steer_gap / 2, steer_gap / 2]    # 记录间隙位置
        self.game_th = 0.0
        self.game_st = 0.0
        self.game_br = 0.0

    # 油门控制
    def thr_control(self, thr):
        thr_diff = thr - self.v / 30.0
        if thr_diff >= 0 and thr != 0 and self.thr != 0:
            self.a = (thr - self.v / 30.0) * 3.0
            if self.a > Max_a:
                self.a = Max_a
        else:
            self.a = -1      # 降低油门时减速控制，以1m/s2的减速度缓慢制动
            if abs(self.a * self.dt) > self.v:
                self.dt = self.v / abs(self.a)
        self.thr = thr

    # 制动控制
    def brake_control(self, brake):
        self.a = -20 * abs(brake)
        self.brake = brake
        if self.a < -Max_a:  # 超过最大附着力，限制最大减速度为Max_a
            self.a = -Max_a
        if abs(self.a * self.dt) > self.v:
            self.dt = self.v / abs(self.a)

    # 加速度控制，简易模拟油门、制动与加速度间比例关系
    def a_control(self, thr, brake):
        if brake != 0:  # 制动优先
            self.brake_control(brake)
            self.thr = thr
            return
        self.thr_control(thr)
        self.brake = brake

    # 适配单片机航向角，y轴为基准顺时针旋转
    def correct_head(self):
        if 90 >= self.ho >= 0:
            self.ho_out = 90 - self.ho
        else:
            self.ho_out = 450 - self.ho

    # 二自由度模型，增加加速度对x,y影响 ，未验证其合理性
    def update(self, a, delta):
        beta = -math.atan(math.tan(delta * math.pi / 180) / 2) * 180 / math.pi
        # print(beta)
        self.x = self.x + (self.v * self.dt + self.a*self.dt*self.dt/2) * math.cos((self.ho+beta)*math.pi/180)
        self.y = self.y + (self.v * self.dt + self.a*self.dt*self.dt/2) * math.sin((self.ho+beta)*math.pi/180)
        # self.x = self.x + self.v * self.dt * math.cos((self.ho + beta)*math.pi/180)
        # self.y = self.y + self.v * self.dt * math.sin((self.ho + beta) * math.pi / 180)
        # self.ho = self.ho + self.v * self.dt * 180 / (L * math.pi) * 2 * math.sin(beta*math.pi/180)
        # R = math.sqrt(L ** 2 * (1 / math.tan(delta * math.pi / 180) ** 2 + 1 / 4))
        if delta != 0:
            L1 = math.sqrt(L ** 2 * (1 / math.tan(delta * math.pi / 180) ** 2 + 1 / 4))
            # L1 = L/math.tan(delta*math.pi/180)
            if delta > 0:
                self.ho = self.ho - (self.v * self.dt + self.a*self.dt*self.dt/2) * 180 / (math.pi * L1)
            elif delta < 0:
                self.ho = self.ho + (self.v * self.dt + self.a * self.dt * self.dt / 2) * 180 / (math.pi * L1)
        self.ho = self.ho
        if self.ho > 360:
            self.ho = self.ho - 360
        elif self.ho < 0:
            self.ho = self.ho + 360
        self.v = self.v + a * self.dt
        self.correct_head()
        self.dt = 1.0 / 60.0          # 重新校对仿真频率

    # 更新行车数据
    def update_control(self, thr, brake, steer):
        if thr > 1:
            thr = 0.999
        elif thr < 0:
            thr = 0
        if brake > 1:
            brake = 0.999
        elif brake < 0:
            brake = 0
        if steer > 1:
            steer = 0.99
        elif steer < -1:
            steer = -0.99
        self.steer = steer
        steer = self.renewal_gap(steer * 45)
        self.a_control(thr, brake)
        self.update(self.a, steer)


    # return to pai
    def back_data(self):
        data = 'x' + str(round(self.x, 3)) + ','
        data = data + str(round(self.y, 3)) + ','
        data = data + str(round(self.ho, 3)) + ','
        data = data + str(round(self.v, 3)) + ','
        data = data + str(int(self.model)) + ','
        data = data + str(round(self.thr, 3)) + ','
        data = data + str(round(self.brake, 3)) + ','
        data = data + str(self.steer) + ','
        data = data + str(int(self.soc)) + '\r\n'
        return data

    # 按键油门控制
    def game_thr(self, a):
        if a == 1:
            self.game_th += 0.002
        elif a == 2:
            self.game_th = self.game_th
        elif a == 0:
            self.game_th = self.thr - 0.005
            if self.game_th < 0:
                self.game_th = 0

    # 角度制 注意转化为[-1～1]
    def game_steer(self, a):
        if a > 0:
            if self.steer >= 0:
                self.game_st += 2
            else:
                self.game_st = 0 + 2
        elif a < 0:
            if self.steer <= 0:
                self.game_st -= 2
            else:
                self.game_st = 0 - 2
        elif a == 0:
            if self.steer*45 >= 10:
                self.game_st = self.steer*45 - 10
            elif self.steer*45 <= -10:
                self.game_st = self.steer*45 + 10
            else:
                self.game_st = 0

    # 按键制动控制
    def game_brake(self):
        self.game_br += 0.005

    # 转向偏差判断
    def steer_deviation(self, st):
        diff = self.bef_steer - st
        if diff > 0:
            if diff <= steer_gap / 2:
                return self.bef_steer
            else:
                return self.bef_steer - steer_gap / 2
        else:
            if diff <= -steer_gap / 2:
                return self.bef_steer + steer_gap / 2
            else:
                return self.bef_steer

    # 更新间隙,返回实际转角
    def renewal_gap(self, st):
        if st < self.bef_s[0]:
            self.bef_s[0] = st
            self.bef_s[1] = self.bef_s[0] + steer_gap
            return self.bef_s[0] + steer_gap / 2
        elif st > self.bef_s[1]:
            self.bef_s[1] = st
            self.bef_s[0] = self.bef_s[1] - steer_gap
            return self.bef_s[1] - steer_gap / 2
        else:
            return self.bef_s[0] + steer_gap / 2
